#include "camdrv.h"
#include "camplus.h"

void TurnYCbCrToRGB565(Uint16 *pYCbCr, Uint16 *pRGB565, int iWidth, int iHeight)
{
    int RGB_R, RGB_G, RGB_B;
    int Y[2], U, V;
    Uint8 *pYUV;
    int i;

    pYUV = (Uint8 *)pYCbCr;

    for (i = 0; i < iWidth*iHeight*2; i+=4) {
        Y[0] = pYUV[i];
        U = pYUV[i + 1];
        Y[1] = pYUV[i + 2];
        V = pYUV[i + 3];

        RGB_R = (int)(1.164*(Y[0] - 16) + 1.596*(V - 128));
        RGB_G = (int)(1.164*(Y[0] - 16) - 0.39*(U - 128) - 0.813*(V - 128));
        RGB_B = (int)(1.164*(Y[0] - 16) + 2.018*(U - 128));

        if (RGB_R < 0) RGB_R = 0;
        if (RGB_R > 0xFF) RGB_R = 0xFF;

        if (RGB_G < 0) RGB_G = 0;
        if (RGB_G > 0xFF) RGB_G = 0xFF;

        if (RGB_B < 0) RGB_B = 0;
        if (RGB_B > 0xFF) RGB_B = 0xFF;

        *pRGB565 =
        ((RGB_R & 0xF8) << 8) | ((RGB_G & 0xFC) << 3) | (RGB_B >> 3);
//        *pRGB565 =
//        ((0xF8) << 8);
        pRGB565++;

        RGB_R = (int)(1.164*(Y[1] - 16) + 1.596*(V - 128));
        RGB_G = (int)(1.164*(Y[1] - 16) - 0.39*(U - 128) - 0.813*(V - 128));
        RGB_B = (int)(1.164*(Y[1] - 16) + 2.018*(U - 128));

        if (RGB_R < 0) RGB_R = 0;
        if (RGB_R > 0xFF) RGB_R = 0xFF;

        if (RGB_G < 0) RGB_G = 0;
        if (RGB_G > 0xFF) RGB_G = 0xFF;

        if (RGB_B < 0) RGB_B = 0;
        if (RGB_B > 0xFF) RGB_B = 0xFF;

        *pRGB565 =
        ((RGB_R & 0xF8) << 8) | ((RGB_G & 0xFC) << 3) | (RGB_B >> 3);
//        *pRGB565 =
//        ((0xF8) << 8);

        pRGB565++;
    }
}

void TurnYCbCrToRGB888(Uint16 *pYCbCr, char *pRGB888, int iWidth, int iHeight)
{
    int RGB_R, RGB_G, RGB_B;
    int Y[2], U, V;
    Uint8 *pYUV;
    int i;

    pYUV = (Uint8 *)pYCbCr;

    for (i = 0; i < iWidth*iHeight*2; i+=4) {
        Y[0] = pYUV[i];
        U = pYUV[i + 1];
        Y[1] = pYUV[i + 2];
        V = pYUV[i + 3];

        RGB_R = (int)(1.164*(Y[0] - 16) + 1.596*(V - 128));
        RGB_G = (int)(1.164*(Y[0] - 16) - 0.39*(U - 128) - 0.813*(V - 128));
        RGB_B = (int)(1.164*(Y[0] - 16) + 2.018*(U - 128));

        if (RGB_R < 0) RGB_R = 0;
        if (RGB_R > 0xFF) RGB_R = 0xFF;

        if (RGB_G < 0) RGB_G = 0;
        if (RGB_G > 0xFF) RGB_G = 0xFF;

        if (RGB_B < 0) RGB_B = 0;
        if (RGB_B > 0xFF) RGB_B = 0xFF;

        *pRGB888 = (unsigned char)RGB_B;
        *(pRGB888 + 1) = (unsigned char)RGB_G;
        *(pRGB888 + 2) = (unsigned char)RGB_R;

        pRGB888+=3;

        RGB_R = (int)(1.164*(Y[1] - 16) + 1.596*(V - 128));
        RGB_G = (int)(1.164*(Y[1] - 16) - 0.39*(U - 128) - 0.813*(V - 128));
        RGB_B = (int)(1.164*(Y[1] - 16) + 2.018*(U - 128));

        if (RGB_R < 0) RGB_R = 0;
        if (RGB_R > 0xFF) RGB_R = 0xFF;

        if (RGB_G < 0) RGB_G = 0;
        if (RGB_G > 0xFF) RGB_G = 0xFF;

        if (RGB_B < 0) RGB_B = 0;
        if (RGB_B > 0xFF) RGB_B = 0xFF;


        *pRGB888 = (unsigned char)RGB_B;
        *(pRGB888 + 1) = (unsigned char)RGB_G;
        *(pRGB888 + 2) = (unsigned char)RGB_R;

        pRGB888+=3;
    }
}

// vertical to horizontal
void V2H(void *pV, void *pH, int iSrcFrameWidth, int iSrcFrameHeight, int iCountBytes)
{
    int x, y;
    int si, di;
    char *pDes;
    char *pSrc;

    pDes = (char *)pH;
    pSrc = (char *)pV;

    for(y = 0; y < iSrcFrameHeight; y++){
        for(x=0; x < iSrcFrameWidth; x++){
            si = iCountBytes * (y * iSrcFrameWidth + x);
            di = iCountBytes * ((iSrcFrameWidth - 1 - x) * iSrcFrameHeight + y);
            memcpy(&pDes[di], &pSrc[si], iCountBytes);
        }
    }
}

void TurnYUV_VToRGB565_H(Uint16 *pYUVSrc, Uint16 *pRGB565Des, int iWidth, int iHeight)
{
    int si, di;
    int x, y;
    int RGB_R, RGB_G, RGB_B;
    int Y[2], U, V;
    Uint16 RGB565[2];
    Uint8 *pYUV;
    int iYUVFrameWidth = iHeight;
    int iYUVFrameHeight = iWidth;
    si = 0;
    for(y = 0, si = 0; y < iYUVFrameHeight; y++){
        for(x=0; x < iYUVFrameWidth; x+=2, si += 2){

            pYUV = (Uint8 *)(pYUVSrc+si);
//            si += 2;
            Y[0] = pYUV[0];
            U = pYUV[1];
            Y[1] = pYUV[2];
            V = pYUV[3];

            RGB_R = (int)(1.164*(Y[0] - 16) + 1.596*(V - 128));
            RGB_G = (int)(1.164*(Y[0] - 16) - 0.39*(U - 128) - 0.813*(V - 128));
            RGB_B = (int)(1.164*(Y[0] - 16) + 2.018*(U - 128));

            if (RGB_R < 0) RGB_R = 0;
            if (RGB_R > 0xFF) RGB_R = 0xFF;

            if (RGB_G < 0) RGB_G = 0;
            if (RGB_G > 0xFF) RGB_G = 0xFF;

            if (RGB_B < 0) RGB_B = 0;
            if (RGB_B > 0xFF) RGB_B = 0xFF;

            RGB565[0] =
            ((RGB_R & 0xF8) << 8) | ((RGB_G & 0xFC) << 3) | (RGB_B >> 3);

            RGB_R = (int)(1.164*(Y[1] - 16) + 1.596*(V - 128));
            RGB_G = (int)(1.164*(Y[1] - 16) - 0.39*(U - 128) - 0.813*(V - 128));
            RGB_B = (int)(1.164*(Y[1] - 16) + 2.018*(U - 128));

            if (RGB_R < 0) RGB_R = 0;
            if (RGB_R > 0xFF) RGB_R = 0xFF;

            if (RGB_G < 0) RGB_G = 0;
            if (RGB_G > 0xFF) RGB_G = 0xFF;

            if (RGB_B < 0) RGB_B = 0;
            if (RGB_B > 0xFF) RGB_B = 0xFF;

            RGB565[1] =
            ((RGB_R & 0xF8) << 8) | ((RGB_G & 0xFC) << 3) | (RGB_B >> 3);

            di = ((iYUVFrameWidth - 1 - x) * iYUVFrameHeight + y);
            pRGB565Des[di] = RGB565[0];
            di = ((iYUVFrameWidth - 2 - x) * iYUVFrameHeight + y);
            pRGB565Des[di] = RGB565[1];
        }
    }

}

//INLINE UInt32 Size2Order(UInt32 size)
//{
//    UInt32 order = 0;
//    UInt32 pageSize = PAGE_SIZE;
//
//    while (pageSize < size) {
//        pageSize <<= 1;
//        order++;
//    }
//
//    return order;
//}

uint_t GetOrder(int iWidth, int iHeight)
{
    uint_t uOrder;
    int size = iWidth*iHeight;
    //1024*1280->2^10;
    //768*1024->2^10;
    //480*640->2^8;
    //240*320->2^6;
    //176*144->2^4

    switch(size){
        case 1200*1600:
            uOrder = 10;
            break;
        case 1024*1280:
            uOrder = 10;
            break;
        case 768*1024:
            uOrder = 10;
            break;
        case 480*640:
            uOrder = 8;
            break;
        case 240*320:
            uOrder = 6 ;
            break;
        case 160*120:
            uOrder = 4;
            break;
        default://240*320
            uOrder = 6;
            break;
    }

    return uOrder;
}


void ResolutionOfSnapshot(int iResolution,
                      int *pWidth,
                      int *pHeight)
{
    switch (iResolution) {
        case 0:
            *pWidth = 120;
            *pHeight = 160;
            break;
        case 1:
            *pWidth = 240;
            *pHeight = 320;
            break;
        case 2:
            *pWidth = 480;
            *pHeight = 640;
            break;
        case 3:
            *pWidth = 768;
            *pHeight = 1024;
            break;
        case 4:
            *pWidth = 1024;
            *pHeight = 1280;
            break;
        case 5:
            *pWidth = 1200;
            *pHeight = 1600;
            break;
        default:
            *pWidth = 240;
            *pHeight = 320;
            break;
    }
}

void ResolutionOfVideo(int iResolution,
                      int *pWidth,
                      int *pHeight)
{
    switch (iResolution) {
        case 0:
            *pWidth = 96;
            *pHeight = 128;
            break;
        case 1:
            *pWidth = 144;
            *pHeight = 176;
            break;
        case 2:
            *pWidth = 240;
            *pHeight = 320;
            break;
        default:
            *pWidth = 240;
            *pHeight = 320;
            break;
    }
}

